from unitree_legged_msgs.msg import MotorCmd
from unitree_legged_msgs.msg import LowCmd
from unitree_legged_msgs.msg import LowState
from config import Config
import copy,rospy
import time

class publisher:
    def __init__(self) -> None:
        # rospy.init_node('joint_control', anonymous=True)
        self.publishers = rospy.Publisher("low_cmd_info", LowCmd, queue_size = 1)
        self.low_cmd=LowCmd()
        for joint_name,index in Config.joint2MotorIndex.items():                   
            self.low_cmd.motorCmd[index].mode = 0x0A   
            self.low_cmd.motorCmd[index].q=0                 
            self.low_cmd.motorCmd[index].dq = 0
            self.low_cmd.motorCmd[index].tau = 0.0
            self.low_cmd.motorCmd[index].Kp = Config.kp
            self.low_cmd.motorCmd[index].Kd = Config.kd
            # self.low_cmd.motorCmd[index].Kp = 85
            # self.low_cmd.motorCmd[index].Kd = 2
    
    def publish_topics_smooth(self,position_dict,duration):
        temp=copy.deepcopy(self.low_cmd)
        # smooth  的逻辑也需要改
        for i in range(1, int(duration) + 1):
            if not rospy.is_shutdown():
                percent = i / duration
                for joint_name,index in Config.joint2MotorIndex.items():
                    self.low_cmd.motorCmd[index].q =temp.motorCmd[index].q * (1 - percent) + position_dict[joint_name] * percent                   
                self.publishers.publish(self.low_cmd)


    def publish_topics(self,position_dict):
        for joint_name,index in Config.joint2MotorIndex.items():
            self.low_cmd.motorCmd[index].q =position_dict[joint_name]            
        self.publishers.publish(self.low_cmd)


    def set_position(self,position_dict):
            for joint_name,index in Config.joint2MotorIndex.items():        
                self.low_cmd.motorCmd[index].q=position_dict[joint_name]